#include "SimEnvironment.h"
/*
SimEnvironment::SimEnvironment() : CTransitionFunction(){

}
*/
SimEnvironment::SimEnvironment(int numContinuous, int numDiscrete, Simworld *pmyworld, int pexptNum)
: CTransitionFunction(new CStateProperties(numContinuous, numDiscrete), new CActionSet())
{
	exptNum = pexptNum;
	myworld = pmyworld;

	//printf("SimEnvironment()\n");

	switch(exptNum){

	case EXPERIMENT_1_SPHERE_GOAL:
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_2_PUSH_BALL:
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_3_JOINT_ACTION_PUSH:
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_4_COOPERATION_OBSTACLE:
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_5_CAPTURE_HUMAN:
	{
		minWorld = opal::Point3r(-12.0, 0.0, -12.0);
		maxWorld = opal::Point3r(12.0, 0.0, 12.0);

		opal::Point3r nonAgentPos = myworld->non_agents[0]->getAgentPosition(0);

		minTerminalState = nonAgentPos - opal::Vec3r(3.0,3.0,3.0);
		maxTerminalState = nonAgentPos + opal::Vec3r(3.0,3.0,3.0);

		max_force = 10;
		max_sim_time = 4;

		numProps= 0;
		for(int i=0; i< numContinuous; i++){
			properties->setMinValue(numProps, minWorld.x - 1.0);
			properties->setMaxValue(numProps, maxWorld.x + 1.0);
			numProps++;
		}

		SetResetPositions();
		break;
	}
	case EXPERIMENT_6_MULTI_AGENT_PUSH:
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_7_JOINTS:
		properties->setDiscreteStateSize(0, 2);
		defaultInit(numContinuous);
		break;

	case EXPERIMENT_8_MULTI_IL_PUSH:
		defaultInit(numContinuous);
		break;

	}

}

void SimEnvironment::defaultInit(int numContinuous){
	minWorld = opal::Point3r(-12.0, 0.0, -12.0);
	maxWorld = opal::Point3r(12.0, 0.0, 12.0);
	minTerminalState = opal::Point3r(9.0, -9.0, -9.0);
	maxTerminalState = opal::Point3r(15.0, 10.0, 15.0);

	max_force = 10;
	max_sim_time = 4;

	numProps= 0;
	for(int i=0; i< numContinuous; i++){
		properties->setMinValue(numProps, minWorld.x - 1.0);
		properties->setMaxValue(numProps, maxWorld.x + 1.0);
		numProps++;
	}

	SetResetPositions();
}

void SimEnvironment::transitionFunction(CState *oldstate, CAction *action, CState *newState, CActionData *data) {

	SimAction *myAct = dynamic_cast<SimAction *>(action);


	switch(exptNum){

	case EXPERIMENT_1_SPHERE_GOAL:
		{
		//printf("transitionFunction()");
		//printf("applied force \n");\

		myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
		// break here for multi agent stuff.
		// wait for all agents to apply force and then only simulate once.
		myworld->simulate(max_sim_time);

		// From now on each agent will run its own.
		//printf("simulated for max sim time \n");

		opal::Point3r creaturePos;
		creaturePos = myworld->agents[0]->getAgentPosition(0);
		newState->setContinuousState(0,creaturePos.x);
		newState->setContinuousState(1,creaturePos.z);

		break;
		}
	case EXPERIMENT_2_PUSH_BALL:
		{
			//printf("transitionFunction()");
			//printf("applied force \n");\

			myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
			// break here for multi agent stuff.
			// wait for all agents to apply force and then only simulate once.
			myworld->simulate(max_sim_time);

			// From now on each agent will run its own.
			//printf("simulated for max sim time \n");

			opal::Point3r creaturePos, creaturePos2;
			creaturePos = myworld->agents[0]->getAgentPosition(0);
			newState->setContinuousState(0,creaturePos.x);
			newState->setContinuousState(1,creaturePos.z);

			creaturePos2 = myworld->non_agents[0]->getAgentPosition(0);
			newState->setContinuousState(2,creaturePos2.x);// - creaturePos.x);
			newState->setContinuousState(3,creaturePos2.z);// - creaturePos.z);

			break;		/*
				 srand ( time(NULL) );
				 double fx = rand() % 20 - 10;
				 double fy = rand() % 20 - 10;
				 double fz = rand() % 20 - 10;

				myworld->non_agents[1]->applyForce(0,opal::Vec3r(fx, fy, fz));
				*/
		}
	case EXPERIMENT_3_JOINT_ACTION_PUSH:
		{
				//printf("transitionFunction()");
				//printf("applied force \n");\

				myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				myworld->agents[0]->applyForce(1,opal::Vec3r(myAct->getActionVal(3), myAct->getActionVal(4), myAct->getActionVal(5)));
				// break here for multi agent stuff.
				// wait for all agents to apply force and then only simulate once.
				myworld->simulate(max_sim_time);

				// From now on each agent will run its own.
				//printf("simulated for max sim time \n");

				opal::Point3r creaturePosSolid0, creaturePosSolid1, creatureNonAgent;
				creaturePosSolid0 = myworld->agents[0]->getAgentPosition(0);
				newState->setContinuousState(0,creaturePosSolid0.x);
				newState->setContinuousState(1,creaturePosSolid0.z);

				creaturePosSolid1 = myworld->agents[0]->getAgentPosition(1);
				newState->setContinuousState(2,creaturePosSolid1.x );
				newState->setContinuousState(3,creaturePosSolid1.z );

				// ball to be pushed
				creatureNonAgent = myworld->non_agents[0]->getAgentPosition(0);
				newState->setContinuousState(4,creatureNonAgent.x );
				newState->setContinuousState(5,creatureNonAgent.z );

				break;		/*
					 srand ( time(NULL) );
					 double fx = rand() % 20 - 10;
					 double fy = rand() % 20 - 10;
					 double fz = rand() % 20 - 10;

					myworld->non_agents[1]->applyForce(0,opal::Vec3r(fx, fy, fz));
					*/
		}

	case EXPERIMENT_4_COOPERATION_OBSTACLE:
		{
				//printf("transitionFunction()");
				//printf("applied force \n");\

				myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				myworld->agents[0]->applyForce(1,opal::Vec3r(myAct->getActionVal(3), myAct->getActionVal(4), myAct->getActionVal(5)));
				// break here for multi agent stuff.
				// wait for all agents to apply force and then only simulate once.
				myworld->simulate(max_sim_time);

				// From now on each agent will run its own.
				//printf("simulated for max sim time \n");

				opal::Point3r creaturePosSolid0, creaturePosSolid1, creatureNonAgent;
				creaturePosSolid0 = myworld->agents[0]->getAgentPosition(0);
				newState->setContinuousState(0,creaturePosSolid0.x);
				newState->setContinuousState(1,creaturePosSolid0.z);

				creaturePosSolid1 = myworld->agents[0]->getAgentPosition(1);
				newState->setContinuousState(2,creaturePosSolid1.x );
				newState->setContinuousState(3,creaturePosSolid1.z );

				opal::Point3r minControlArea = opal::Point3r( -12, -5, 4);
				opal::Point3r maxControlArea = opal::Point3r( -4, +5, 12);

				bool removeObstacle = false;
				double x,y,z;
				x = creaturePosSolid1.x ;
				y = creaturePosSolid1.y ;
				z = creaturePosSolid1.z ;
				if( (x >= minControlArea.x && y >= minControlArea.y && z >= minControlArea.z)
					&&	(x <= maxControlArea.x && y <= maxControlArea.y && z <= maxControlArea.z)
					){
					removeObstacle = true;
				}

				x = creaturePosSolid0.x ;
				y = creaturePosSolid0.y ;
				z = creaturePosSolid0.z ;
				if( (x >= minControlArea.x && y >= minControlArea.y && z >= minControlArea.z)
					&&	(x <= maxControlArea.x && y <= maxControlArea.y && z <= maxControlArea.z)
					){
					removeObstacle = true;
				}

				opal::Point3r outsidePt = opal::Point3r(20,20,20);
				if(removeObstacle) {
					//std::cout<<" removed obstacle "<<std::endl;
					myworld->non_agents[0]->setAgentPosition(0, outsidePt);
				} else {
					myworld->non_agents[0]->resetPositions(minWorld, maxWorld);
				}

				break;
		}
	case EXPERIMENT_5_CAPTURE_HUMAN:
		{
				//printf("transitionFunction()");
				//printf("applied force \n");\

				myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				// break here for multi agent stuff.
				// wait for all agents to apply force and then only simulate once.
				myworld->simulate(max_sim_time);

				// From now on each agent will run its own.
				//printf("simulated for max sim time \n");

				opal::Point3r creaturePosSolid0, creatureNonAgent;
				creaturePosSolid0 = myworld->agents[0]->getAgentPosition(0);
				newState->setContinuousState(0,creaturePosSolid0.x);
				newState->setContinuousState(1,creaturePosSolid0.z);

				creatureNonAgent = myworld->non_agents[0]->getAgentPosition(0);
				newState->setContinuousState(2,creatureNonAgent.x );
				newState->setContinuousState(3,creatureNonAgent.z );

				minTerminalState = creatureNonAgent - opal::Vec3r(3.0,3.0,3.0);
				maxTerminalState = creatureNonAgent + opal::Vec3r(3.0,3.0,3.0);
				break;
		}
	case EXPERIMENT_6_MULTI_AGENT_PUSH:
		{
			//	printf("transitionFunction()");


				if(myAct->getActionVal(3) == 0){
					myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				} else 	if(myAct->getActionVal(3) == 1){
					myworld->agents[1]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				}

			//	std::cout<<"applied force "<<myAct->getActionVal(3)<<" threadid: "<< pthread_self() <<std::endl;

			//	std::cout<< "threadid: "<< pthread_self() <<std::endl;
				//printf(" my world : %u \n", myworld);
				int r=pthread_barrier_wait(&simBarrier);



				//myworld->agents[0]->applyForce(1,opal::Vec3r(myAct->getActionVal(3), myAct->getActionVal(4), myAct->getActionVal(5)));
				// break here for multi agent stuff.
				// wait for all agents to apply force and then only simulate once.
				//if(myAct->getActionVal(3) == 0){
				if(mainThreadId == pthread_self()) {
					//std::cout<<" HERE IS BARRIER"<<"threadid: "<< pthread_self() <<std::endl;
					myworld->simulate(max_sim_time);
				}


				//std::cout<< "threadid: "<< pthread_self() <<std::endl;
				r=pthread_barrier_wait(&simBarrier);

				// From now on each agent will run its own.
//				printf("simulated for max sim time \n");

				opal::Point3r creaturePosSolid0, creaturePosSolid1, creatureNonAgent;

				creaturePosSolid0 = myworld->agents[0]->getAgentPosition(0);
				newState->setContinuousState(0,creaturePosSolid0.x);
				newState->setContinuousState(1,creaturePosSolid0.z);

				creaturePosSolid1 = myworld->agents[1]->getAgentPosition(0);
				newState->setContinuousState(2,creaturePosSolid1.x );
				newState->setContinuousState(3,creaturePosSolid1.z );


				creatureNonAgent = myworld->non_agents[0]->getAgentPosition(0);
				newState->setContinuousState(4,creatureNonAgent.x );
				newState->setContinuousState(5,creatureNonAgent.z );
				/*
				if(mainThreadId == pthread_self()) {
					std :: cout << " Thread ID :: " << pthread_self() << " Agent 0 Y Position:: " <<creaturePosSolid0.y<<std::endl;
					std :: cout << " Thread ID :: " << pthread_self() << " Agent 1 Y Position:: " <<creaturePosSolid1.y<<std::endl;
					opal::SolidData data =  myworld -> non_agents[1] -> solids[0]->getData();
					opal::ShapeData *shapeData = data.getShapeData(0);

					//std :: cout << " Thread ID :: "<< pthread_self() << " Box density:: " << shapeData->material.density << std::endl;
				}*/

			break;
		}
	case EXPERIMENT_7_JOINTS:
		{
			//printf("transitionFunction()");
			//printf("applied force \n");\

			//if( oldstate->getDiscreteState(0) == 0 ) {
			if(myAct->getActionVal(0) == 0){
				myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(1), myAct->getActionVal(2), myAct->getActionVal(3)));
			} else {
				//myworld->agents[0]->applyGearedForce(0, myAct->getActionVal(1));
				myworld->agents[0]->applyServoForce(0, myAct->getActionVal(1));
				//myworld->agents[0]->applyGearedForce(0, 0.9);

				//myworld->agents[0]->applyForce(1,opal::Vec3r(myAct->getActionVal(1) * 2, myAct->getActionVal(2) * 2, myAct->getActionVal(3) * 2));
			}

			// break here for multi agent stuff.
			// wait for all agents to apply force and then only simulate once.
			myworld->simulate(max_sim_time);

			// From now on each agent will run its own.
			//printf("simulated for max sim time \n");

			opal::Point3r creaturePos, creaturePos2;
			creaturePos = myworld->agents[0]->getAgentPosition(0);
			newState->setContinuousState(0,creaturePos.x);
			newState->setContinuousState(1,creaturePos.z);


			creaturePos2 = myworld->agents[0]->getAgentPosition(1);


			if((creaturePos - creaturePos2).length() <= 5){
				myworld->agents[0]->joinSolids(0,1);
				newState->setDiscreteState(0, 1);
			} else {
				myworld->agents[0]->breakJoints(0);
				newState->setDiscreteState(0, 0);
			}


			opal::Point3r creatureNonAgentPos;
			creatureNonAgentPos = myworld->non_agents[0]->getAgentPosition(0);
			newState->setContinuousState(2,creatureNonAgentPos.x );
			newState->setContinuousState(3,creatureNonAgentPos.z );


			break;		/*
				 srand ( time(NULL) );
				 double fx = rand() % 20 - 10;
				 double fy = rand() % 20 - 10;
				 double fz = rand() % 20 - 10;

				myworld->non_agents[1]->applyForce(0,opal::Vec3r(fx, fy, fz));
				*/
		}
		case EXPERIMENT_8_MULTI_IL_PUSH:
		{
			//	printf("transitionFunction()");


				if(myAct->getActionVal(3) == 0){
					myworld->agents[0]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				} else 	if(myAct->getActionVal(3) == 1){
					myworld->agents[1]->applyForce(0,opal::Vec3r(myAct->getActionVal(0), myAct->getActionVal(1), myAct->getActionVal(2)));
				}

			//	std::cout<<"applied force "<<myAct->getActionVal(3)<<" threadid: "<< pthread_self() <<std::endl;

			//	std::cout<< "threadid: "<< pthread_self() <<std::endl;
				//printf(" my world : %u \n", myworld);
				int r=pthread_barrier_wait(&simBarrier);



				//myworld->agents[0]->applyForce(1,opal::Vec3r(myAct->getActionVal(3), myAct->getActionVal(4), myAct->getActionVal(5)));
				// break here for multi agent stuff.
				// wait for all agents to apply force and then only simulate once.
				//if(myAct->getActionVal(3) == 0){
				if(mainThreadId == pthread_self()) {
					//std::cout<<" HERE IS BARRIER"<<"threadid: "<< pthread_self() <<std::endl;
					myworld->simulate(max_sim_time);
				}


				//std::cout<< "threadid: "<< pthread_self() <<std::endl;
				r=pthread_barrier_wait(&simBarrier);

				// From now on each agent will run its own.
//				printf("simulated for max sim time \n");

				opal::Point3r creaturePosSolid0, creaturePosSolid1, creatureNonAgent;

				if(myAct->getActionVal(3) == 0){
					creaturePosSolid0 = myworld->agents[0]->getAgentPosition(0);
					newState->setContinuousState(0,creaturePosSolid0.x);
					newState->setContinuousState(1,creaturePosSolid0.z);
				} else if(myAct->getActionVal(3) == 1){
					creaturePosSolid1 = myworld->agents[1]->getAgentPosition(0);
					newState->setContinuousState(0,creaturePosSolid1.x );
					newState->setContinuousState(1,creaturePosSolid1.z );
				}

				creatureNonAgent = myworld->non_agents[0]->getAgentPosition(0);
				newState->setContinuousState(2,creatureNonAgent.x );
				newState->setContinuousState(3,creatureNonAgent.z );
				/*
				if(mainThreadId == pthread_self()) {
					std :: cout << " Thread ID :: " << pthread_self() << " Agent 0 Y Position:: " <<creaturePosSolid0.y<<std::endl;
					std :: cout << " Thread ID :: " << pthread_self() << " Agent 1 Y Position:: " <<creaturePosSolid1.y<<std::endl;
					opal::SolidData data =  myworld -> non_agents[1] -> solids[0]->getData();
					opal::ShapeData *shapeData = data.getShapeData(0);

					//std :: cout << " Thread ID :: "<< pthread_self() << " Box density:: " << shapeData->material.density << std::endl;
				}*/

			break;
		}
	//	std::cout<<"transitionfn finished  "<<std::endl;
	}
}

bool SimEnvironment::isFailedState(CState *s){

	bool is_failed = false;

	switch(exptNum){

	case EXPERIMENT_1_SPHERE_GOAL:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_2_PUSH_BALL:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(2);// + s->getContinuousState(0);
		z = s->getContinuousState(3);// + s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_3_JOINT_ACTION_PUSH:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(2);
		z = s->getContinuousState(3);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(4);
		z = s->getContinuousState(5);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_4_COOPERATION_OBSTACLE:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(2);
		z = s->getContinuousState(3);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_5_CAPTURE_HUMAN:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(2);
		z = s->getContinuousState(3);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}

	case EXPERIMENT_6_MULTI_AGENT_PUSH:
	{
		is_failed = false;
		int r=pthread_barrier_wait(&simBarrier);

		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
			//std::cout<<" agent 0 failed "<<std::endl;
		}

		x = s->getContinuousState(2);
		z = s->getContinuousState(3);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
			//std::cout<<" agent 1 failed "<<std::endl;
		}

		x = s->getContinuousState(4);
		z = s->getContinuousState(5);

		// if non agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
			std::cout<<" non agent 0 failed "<<std::endl;
		}

		r=pthread_barrier_wait(&simBarrier);
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_7_JOINTS:
	{
		double x = s->getContinuousState(0);
		double z = s->getContinuousState(1);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		opal::Point3r boxPos = myworld->agents[0]->getAgentPosition(1);
		x = boxPos.x;
		z = boxPos.z;

		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}

		x = s->getContinuousState(2);
		z = s->getContinuousState(3);

		// if agent falls off the world episode ends in a terminal state.
		if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
			is_failed = true;
		}
		if(is_failed) {
			resetEnvironment();
		}
		break;
	}
	case EXPERIMENT_8_MULTI_IL_PUSH:
		{
			simEnvFailed = false;
			int r=pthread_barrier_wait(&simBarrier);

			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
				simEnvFailed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
				simEnvFailed = true;
			}

			r=pthread_barrier_wait(&simBarrier);

			if(simEnvFailed) {
				is_failed = simEnvFailed;
				resetEnvironment();
			}
/*
			if(is_failed) {
				resetEnvironment();
			}*/
			break;
		}
	}
	return is_failed;
}

bool SimEnvironment::isThreadFailedState(CState *s){

	bool is_failed = false;
	switch(exptNum) {
		case EXPERIMENT_1_SPHERE_GOAL:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_2_PUSH_BALL:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);// + s->getContinuousState(0);
			z = s->getContinuousState(3);// + s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_3_JOINT_ACTION_PUSH:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(4);
			z = s->getContinuousState(5);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_4_COOPERATION_OBSTACLE:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_5_CAPTURE_HUMAN:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_6_MULTI_AGENT_PUSH:
		{
			//int r=pthread_barrier_wait(&simBarrier);

			double x;
			double z;

			if(mainThreadId == pthread_self()){
				x= s->getContinuousState(0);
				z= s->getContinuousState(1);

				// if agent falls off the world episode ends in a terminal state.
				if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
					is_failed = true;
				}

			} else {

				x = s->getContinuousState(2);
				z = s->getContinuousState(3);

				// if agent falls off the world episode ends in a terminal state.
				if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
					is_failed = true;
				}
			}
			x = s->getContinuousState(4);
			z = s->getContinuousState(5);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			break;
		}
		case EXPERIMENT_7_JOINTS:
		{
			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			opal::Point3r boxPos = myworld->agents[0]->getAgentPosition(1);
			x = boxPos.x;
			z = boxPos.z;

			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
		}
		case EXPERIMENT_8_MULTI_IL_PUSH: {
		//	int r=pthread_barrier_wait(&simBarrier);

			double x = s->getContinuousState(0);
			double z = s->getContinuousState(1);

					// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}

			x = s->getContinuousState(2);
			z = s->getContinuousState(3);

			// if agent falls off the world episode ends in a terminal state.
			if(x >= maxWorld.x || x<= minWorld.x || z >= maxWorld.z || z <= minWorld.z) {
				is_failed = true;
			}
			break;
			}

	}
	return is_failed;
}

bool SimEnvironment::isResetState(CState *s){

	switch(exptNum) {
	case EXPERIMENT_1_SPHERE_GOAL: {
		double x = 0.0;
		double y = 0.0;
		double z = 0.0;
		x = s->getContinuousState(0);
		y = 2.5;
		z = s->getContinuousState(1);
		if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
				&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
			) {
				//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

		return false;

	break;
	}
	case EXPERIMENT_2_PUSH_BALL: {
			double x = 0.0;
			double y = 0.0;
			double z = 0.0;
			x = s->getContinuousState(2);// + s->getContinuousState(0);
			y = 2.5;
			z = s->getContinuousState(3);// + s->getContinuousState(1);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			return false;
		break;
	}
	case EXPERIMENT_3_JOINT_ACTION_PUSH: {
			double x = 0.0;
			double y = 0.0;
			double z = 0.0;
			x = s->getContinuousState(4);
			y = 2.5;
			z = s->getContinuousState(5);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			return false;
		break;
	}

	case EXPERIMENT_4_COOPERATION_OBSTACLE: {

			double x = 0.0;
			double y = 0.0;
			double z = 0.0;
			x = s->getContinuousState(0);
			y = 2.5;
			z = s->getContinuousState(1);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			x = s->getContinuousState(2);
			y = 2.5;
			z = s->getContinuousState(3);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			return false;
		break;
	}
	case EXPERIMENT_5_CAPTURE_HUMAN: {

		double x = 0.0;
		double y = 0.0;
		double z = 0.0;
		x = s->getContinuousState(0);
		y = 2.5;
		z = s->getContinuousState(1);
		if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
		   && (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)) {
				//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
		}

		return false;
		break;
	}
	case EXPERIMENT_6_MULTI_AGENT_PUSH: {
			double x = 0.0;
			double y = 0.0;
			double z = 0.0;

		//	int r=pthread_barrier_wait(&simBarrier);
			x = s->getContinuousState(4);
			y = 2.5;
			z = s->getContinuousState(5);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			return false;
		break;
	}
	case EXPERIMENT_7_JOINTS: {
			double x = 0.0;
			double y = 0.0;
			double z = 0.0;
			x = s->getContinuousState(0);
			y = 2.5;
			z = s->getContinuousState(1);
/*
			bool ballReached = false;
			bool boxReached = false;

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";

				ballReached = true;
			}
*/
			x = s->getContinuousState(2);
			y = 2.5;
			z = s->getContinuousState(3);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
					resetEnvironment();
					return true;
					//boxReached = true;
			}
/*
			if(boxReached ){
				resetEnvironment();
				return true;
			}
*/
			return false;
		break;
	 }
	case EXPERIMENT_8_MULTI_IL_PUSH: {

			double x = 0.0;
			double y = 0.0;
			double z = 0.0;

			x = s->getContinuousState(2);
			y = 2.5;
			z = s->getContinuousState(3);

			// if the non agent falls off the world reset its position.
			if((x >= minTerminalState.x && y >= minTerminalState.y && z >= minTerminalState.z)
					&& (x <= maxTerminalState.x && y <= maxTerminalState.y && z <= maxTerminalState.z)
				) {
					//std::cout<<"terminal:: "<<x<<" "<<y<<" "<<z<<" \n";
				resetEnvironment();
				return true;
			}

			return false;
		break;
	}
	}
}

void SimEnvironment::resetEnvironment(){

	int r=pthread_barrier_wait(&simBarrier);
	if(mainThreadId == pthread_self()){
		myworld->simulate(max_sim_time*5);
	}
	r=pthread_barrier_wait(&simBarrier);

	switch(exptNum){
	case EXPERIMENT_1_SPHERE_GOAL:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_2_PUSH_BALL:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_3_JOINT_ACTION_PUSH:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_4_COOPERATION_OBSTACLE:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_5_CAPTURE_HUMAN:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_6_MULTI_AGENT_PUSH:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_7_JOINTS:
		defaultResetEnvironment();
		break;
	case EXPERIMENT_8_MULTI_IL_PUSH:
		defaultResetEnvironment();
		break;
	}

}
void SimEnvironment::defaultResetEnvironment(){
	for(int j=0; j< myworld->agents.size(); j++ ) {
			Creature *c = myworld->agents[j];
			c->resetPositions(minWorld, maxWorld);
	}
	for(int j=0; j< myworld->non_agents.size(); j++ ) {
			Creature *c = myworld->non_agents[j];
			c->resetPositions(minWorld, maxWorld);
	}
}

void SimEnvironment::SetResetPositions(){

	switch(exptNum){
	case EXPERIMENT_1_SPHERE_GOAL:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_2_PUSH_BALL:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_3_JOINT_ACTION_PUSH:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_4_COOPERATION_OBSTACLE:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_5_CAPTURE_HUMAN:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_6_MULTI_AGENT_PUSH:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_7_JOINTS:
		defaultSetResetPositions();
		break;

	case EXPERIMENT_8_MULTI_IL_PUSH:
		defaultSetResetPositions();
		break;
	}

}

void SimEnvironment::defaultSetResetPositions(){
	for(int j=0; j< myworld->agents.size(); j++ ) {
			Creature *c = myworld->agents[j];
			c->setStartStates();
	}
	for(int j=0; j< myworld->non_agents.size(); j++ ) {
			Creature *c = myworld->non_agents[j];
			c->setStartStates();
	}
}

void SimEnvironment::getResetState(CState *resetState){

	opal::Point3r creaturePos;

	switch(exptNum){

	case EXPERIMENT_1_SPHERE_GOAL:
		{

			int temNumProps = 0;
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);
			resetState->setContinuousState(temNumProps,creaturePos.x);
			temNumProps++;
			resetState->setContinuousState(temNumProps,creaturePos.z);
			temNumProps++;
			break;
		}

	case EXPERIMENT_2_PUSH_BALL:
		{
		    Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);

			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
            //std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			break;
		}
	case EXPERIMENT_3_JOINT_ACTION_PUSH:
		{
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);

			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->agents[0];
			creaturePos = c->getStartState(1);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(4,creaturePos.x);
			resetState->setContinuousState(5,creaturePos.z);
			break;
		}

	case EXPERIMENT_4_COOPERATION_OBSTACLE:
		{
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);

			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->agents[0];
			creaturePos = c->getStartState(1);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			break;
		}
	case EXPERIMENT_5_CAPTURE_HUMAN:
		{
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);
			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			break;
		}
	case EXPERIMENT_6_MULTI_AGENT_PUSH:
		{
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);

			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->agents[1];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(4,creaturePos.x);
			resetState->setContinuousState(5,creaturePos.z);
			break;
		}
	case EXPERIMENT_7_JOINTS:
		{
			Creature  *c  = myworld->agents[0];
			creaturePos = c->getStartState(0);

			resetState->setContinuousState(0,creaturePos.x);
			resetState->setContinuousState(1,creaturePos.z);

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			resetState->setDiscreteState(0, 0);

			break;
		}

	case EXPERIMENT_8_MULTI_IL_PUSH:
		{
			Creature  *c;

			if(mainThreadId == pthread_self()){
				c= myworld->agents[0];
				creaturePos = c->getStartState(0);
				resetState->setContinuousState(0,creaturePos.x);
				resetState->setContinuousState(1,creaturePos.z);
			} else {
				c= myworld->agents[1];
				creaturePos = c->getStartState(0);
				resetState->setContinuousState(0,creaturePos.x);
				resetState->setContinuousState(1,creaturePos.z);
			}

			c  = myworld->non_agents[0];
			creaturePos = c->getStartState(0);
			//std::cout <<"Non-agent position  "<<creaturePos.x<< "  "<<creaturePos.y<<"  ";
			resetState->setContinuousState(2,creaturePos.x);
			resetState->setContinuousState(3,creaturePos.z);

			//resetState->setDiscreteState(0, 0);

			break;
		}
	}

}

opal::Point3r SimEnvironment::getMinTerminalState(){
	return minTerminalState;
}

opal::Point3r SimEnvironment::getMaxTerminalState(){
	return maxTerminalState;
}

opal::Point3r SimEnvironment::getMinWorldState(){
	return minWorld;
}

opal::Point3r SimEnvironment::getMaxWorldState(){
	return maxWorld;
}

int SimEnvironment::getNumProperties(){
	return numProps;
}
